
#include "QMC5883.h"
#include <sensor.h>
#include <rtthread.h>

/// @brief 检查传入的结构体指针是否为空
/// @param dev
/// @return 空返回-1，非空返回0
static int8_t null_ptr_check(const qmc5883_dev *dev);

/// @brief 检查自检是否满足要求
/// @param before 自检之前读取地磁数据
/// @param after 自检之后读取地磁数据
/// @return 通过返回0，有一个空指针返回-1，不通过返回
static int8_t check_self_test(qmc5883_mag_data_t *before, qmc5883_mag_data_t *after);

/// @brief 根据QMC5883L数据手册，寄存器0x0B最好写入0x01
/// @param dev
/// @return
static int8_t qmc5883L_default_set(qmc5883_dev *dev);

int8_t qmc5883_init(qmc5883_dev *dev)
{
    int8_t rslt;
    uint8_t try = 5;

    /* Null-pointer check */
    rslt = null_ptr_check(dev);
    if (rslt == QMC5883_OK)
    {
        /* Assign chip id as zero */
        dev->chip_id = 0;
        dev->chip_type = QMC5883L;

        /* first check if device is QMC5883L */
        while (try-- && dev->chip_id != QMC5883L_CHIP_ID)
        {
            /* Read chip_id */
            rslt = qmc5883_get_regs(QMC5883L_CHIP_ID_ADDR, &dev->chip_id, 1, dev);
            dev->delay_ms(3);
        }
        if (dev->chip_id != QMC5883L_CHIP_ID)
        {
            /* then check if device is QMC5883L */
            try = 5;

            while (try-- && dev->chip_id != QMC5883P_CHIP_ID)
            {
                /* Read chip_id */
                rslt = qmc5883_get_regs(QMC5883P_CHIP_ID_ADDR, &dev->chip_id, 1, dev);
                dev->delay_ms(3);
            }

            if (dev->chip_id != QMC5883P_CHIP_ID)
            {
                dev->chip_type = UNKNOWN;
            }
            else
            {
                dev->chip_type = QMC5883P;
                rt_kprintf("find QMC5883P device\n");
            }
        }
        else
        {
            rt_kprintf("find QMC5883L device\n");
        }

        if (rslt == QMC5883_OK && (dev->chip_id == QMC5883P_CHIP_ID || dev->chip_id == QMC5883L_CHIP_ID))
        {
            rslt = qmc5883_soft_reset(dev);
            dev->delay_ms(50);
        }
        else
        {
            rslt = QMC5883_E_DEV_NOT_FOUND;
        }
    }
    return rslt;
}

int8_t qmc5883_get_regs(uint8_t reg_addr, uint8_t *data, uint16_t len, qmc5883_dev *dev)
{
    /* Variable to define error */
    int8_t rslt;

    /* Null-pointer check */
    rslt = null_ptr_check(dev);
    if ((rslt == QMC5883_OK) && (data != NULL))
    {
        dev->intf_rslt = dev->read(reg_addr, data, len);

        if (dev->intf_rslt == QMC5883_INTF_RET_SUCCESS)
        {
            rslt = QMC5883_OK;
        }
        else
        {
            rslt = QMC5883_E_COM_FAIL;
        }
    }
    else
    {
        rslt = QMC5883_E_NULL_PTR;
    }

    return rslt;
}

int8_t qmc5883_set_regs(uint8_t reg_addr, uint8_t *data, uint16_t len, qmc5883_dev *dev)
{
    /* Variable to define error */
    int8_t rslt;

    /* Null-pointer check */
    rslt = null_ptr_check(dev);
    if ((rslt == QMC5883_OK) && (data != NULL))
    {
        dev->intf_rslt = dev->write(reg_addr, data, len);
        dev->delay_ms(3);
        if (dev->intf_rslt == QMC5883_INTF_RET_SUCCESS)
        {
            rslt = QMC5883_OK;
        }
        else
        {
            rslt = QMC5883_E_COM_FAIL;
        }
    }
    else
    {
        rslt = QMC5883_E_NULL_PTR;
    }

    return rslt;
}

int8_t qmc5883_soft_reset(qmc5883_dev *dev)
{
    /* Variable to define error */
    int8_t rslt;
    uint8_t ctrl_status = 0;

    rslt = null_ptr_check(dev);
    if (rslt == QMC5883_OK)
    {
        if (dev->chip_type == QMC5883L)
        {
            /* step1 : read register */
            rslt = qmc5883_get_regs(QMC5883L_CONTROL2_ADDR, &ctrl_status, 1, dev);
            if (rslt != QMC5883_OK)
            {
                rslt = QMC5883_E_COM_FAIL;
                return rslt;
            }

            ctrl_status |= 0x80; // SOFT_RST bit set 1
            ctrl_status &= 0xfc; // DISABLE interrupt pin
            rslt = qmc5883_set_regs(QMC5883L_CONTROL2_ADDR, &ctrl_status, 1, dev);
            if (rslt != QMC5883_OK)
            {
                rslt = QMC5883_E_COM_FAIL;
                return rslt;
            }
        }
        else if (dev->chip_type == QMC5883P)
        {
            /* step1 : read register */
            rslt = qmc5883_get_regs(QMC5883P_CONTROL2_ADDR, &ctrl_status, 1, dev);
            if (rslt != QMC5883_OK)
            {
                rslt = QMC5883_E_COM_FAIL;
                return rslt;
            }

            ctrl_status |= 0x80; // SOFT_RST bit set 1
            ctrl_status &= 0xfc; // SET/RESET bit 1-0 set 2'b00, means set commencement immediately with offset update
            rslt = qmc5883_set_regs(QMC5883P_CONTROL2_ADDR, &ctrl_status, 1, dev);
            if (rslt != QMC5883_OK)
            {
                rslt = QMC5883_E_COM_FAIL;
                return rslt;
            }
        }
    }
    else
    {
        rslt = QMC5883_E_NULL_PTR;
    }

    return rslt;
}

int8_t qmc5883_self_test(qmc5883_dev *dev)
{
    int8_t rslt; /* Variable to define error */
    uint8_t ctrl_status = 0;
    qmc5883_mag_data_t mag_data_before_self_test;
    qmc5883_mag_data_t mag_data_after_self_test;

    rslt = null_ptr_check(dev);
    if (rslt == QMC5883_OK)
    {
        if (dev->chip_type == QMC5883P)
        {
            /* step1 : read register 0x0b */
            rslt = qmc5883_get_regs(QMC5883P_CONTROL2_ADDR, &ctrl_status, 1, dev);
            if (rslt != QMC5883_OK)
            {
                rslt = QMC5883_E_COM_FAIL;
                return rslt;
            }

            /* step2 : read mag data before start test */
            rslt = qmc5883_get_sensor_data(&mag_data_before_self_test, dev);

            /* step3 : update register 0x0b start self test */
            ctrl_status |= 0x40; // SOFT_RST bit set 1
            ctrl_status &= 0xfc; // SET/RESET bit 1-0 set 2'b00, means set commencement immediately with offset update
            rslt = qmc5883_set_regs(QMC5883P_CONTROL2_ADDR, &ctrl_status, 1, dev);

            dev->delay_ms(5); /* wait */

            /* step4 : read mag data before after test */
            rslt = qmc5883_get_sensor_data(&mag_data_after_self_test, dev);

            /* step5 : check self test result */
            rslt = check_self_test(&mag_data_before_self_test, &mag_data_after_self_test);
        }
        else
        {
            rslt = QMC5883_E_INVALID_SENSOR; /* only QMC5883P support self test operation */
        }
    }
    else
    {
        rslt = QMC5883_E_NULL_PTR;
    }

    return rslt;
}

int8_t qmc5883_select_mode(enum QMC5883_Mode mode, qmc5883_dev *dev)
{
    int8_t rslt; /* Variable to define error */
    uint8_t ctrl_status = 0;

    if (dev->chip_type == QMC5883L)
    {
        rslt = qmc5883_get_regs(QMC5883L_CONTROL1_ADDR, &ctrl_status, 1, dev);
    }
    else if (dev->chip_type == QMC5883P)
    {
        rslt = qmc5883_get_regs(QMC5883P_CONTROL1_ADDR, &ctrl_status, 1, dev);
    }

    if (rslt != QMC5883_OK)
    {
        rslt = QMC5883_E_COM_FAIL;
        return rslt;
    }

    switch (mode)
    {
    case suspend:
        /* update reg mode bit field */
        ctrl_status &= 0xfc; /* mode bit 1-0 set 2'b00, means suspend mode */
        break;

    case normal:
        /* update reg mode bit field */
        ctrl_status &= 0xfc;
        ctrl_status |= 0x01; /* mode bit 1-0 set 2'b01, means normal mode */
        break;

    case single:
        /* update reg mode bit field */
        ctrl_status &= 0xfc;
        ctrl_status |= 0x02; /* mode bit 1-0 set 2'b10, means single mode */
        break;

    case continus:
        /* update reg mode bit field */
        ctrl_status &= 0xfc;
        ctrl_status |= 0x03; /* mode bit 1-0 set 2'b11, means continus mode */
        break;

    default:
        break;
    }

    if (dev->chip_type == QMC5883L)
    {
        rslt = qmc5883_set_regs(QMC5883L_CONTROL1_ADDR, &ctrl_status, 1, dev);
    }
    else if (dev->chip_type == QMC5883P)
    {
        rslt = qmc5883_set_regs(QMC5883P_CONTROL1_ADDR, &ctrl_status, 1, dev);
    }

    return rslt;
}

int8_t qmc5883_select_range(enum QMC5883_Range range, qmc5883_dev *dev)
{
    int8_t rslt; /* Variable to define error */
    uint8_t ctrl_status = 0;

    if (dev->chip_type == QMC5883L)
    {
        rslt = qmc5883_get_regs(QMC5883L_CONTROL1_ADDR, &ctrl_status, 1, dev);
        switch (range)
        {
        case range_2_Gauss:
            /* update reg mode range field */
            ctrl_status &= 0xcf; /* range bit 4-5 set 2'b00, means 30Gauss */
            break;

        case range_8_Gauss:
            ctrl_status &= 0xcf;
            ctrl_status |= 0x10; /* range bit 4-5 set 2'b01, means 8Gauss */
            break;

        default:
            break;
        }
    }

    else if (dev->chip_type == QMC5883P)
    {
        rslt = qmc5883_get_regs(QMC5883P_CONTROL2_ADDR, &ctrl_status, 1, dev);
        switch (range)
        {
        case range_30_Gauss:
            /* update reg mode range field */
            ctrl_status &= 0xf3; /* range bit 1-0 set 2'b00, means 30Gauss */
            break;

        case range_12_Gauss:
            ctrl_status &= 0xf3;
            ctrl_status |= 0x04; /* range bit 1-0 set 2'b01, means 12Gauss */
            break;

        case range_8_Gauss:
            ctrl_status &= 0xf3;
            ctrl_status |= 0x08; /* range bit 1-0 set 2'b10, means 8Gauss */
            break;

        case range_2_Gauss:
            ctrl_status &= 0xf3;
            ctrl_status |= 0x0c; /* range bit 1-0 set 2'b11, means 2Gauss */
            break;

        default:
            break;
        }
    }

    if (dev->chip_type == QMC5883L)
    {
        rslt = qmc5883_set_regs(QMC5883L_CONTROL1_ADDR, &ctrl_status, 1, dev);
    }
    else if (dev->chip_type == QMC5883P)
    {
        rslt = qmc5883_set_regs(QMC5883P_CONTROL2_ADDR, &ctrl_status, 1, dev);
    }

    if (rslt != QMC5883_OK)
    {
        rslt = QMC5883_E_COM_FAIL;
        return rslt;
    }

    return rslt;
}

int8_t qmc5883_select_odr(enum QMC5883_odr odr, qmc5883_dev *dev)
{
    int8_t rslt; /* Variable to define error */
    uint8_t ctrl_status = 0;

    if (dev->chip_type == QMC5883L)
    {
        rslt = qmc5883_get_regs(QMC5883L_CONTROL1_ADDR, &ctrl_status, 1, dev);
    }

    else if (dev->chip_type == QMC5883P)
    {
        rslt = qmc5883_get_regs(QMC5883P_CONTROL1_ADDR, &ctrl_status, 1, dev);
    }

    /* because QMC5883L and QMC5883P ODR bit field is same, bit 2~3 */
    switch (odr)
    {
    case ODR_10Hz:
        /* update reg odr bit field */
        ctrl_status &= 0xf3; /* odr bit 1-0 set 2'b00, means 10Hz */
        break;

    case ODR_50Hz:
        /* update reg mode bit field */
        ctrl_status &= 0xf3;
        ctrl_status |= 0x04; /* odr bit 1-0 set 2'b01, means 50Hz */
        break;

    case ODR_100Hz:
        /* update reg mode bit field */
        ctrl_status &= 0xf3;
        ctrl_status |= 0x08; /* odr bit 1-0 set 2'b10, means 100Hz */
        break;

    case ODR_200Hz:
        /* update reg mode bit field */
        ctrl_status &= 0xf3;
        ctrl_status |= 0x0c; /* odr bit 1-0 set 2'b11, means 200Hz */
        break;

    default:
        break;
    }

    if (dev->chip_type == QMC5883L)
    {
        rslt = qmc5883_set_regs(QMC5883L_CONTROL1_ADDR, &ctrl_status, 1, dev);
    }
    else if (dev->chip_type == QMC5883P)
    {
        rslt = qmc5883_set_regs(QMC5883P_CONTROL1_ADDR, &ctrl_status, 1, dev);
    }

    if (rslt != QMC5883_OK)
    {
        rslt = QMC5883_E_COM_FAIL;
        return rslt;
    }

    return rslt;
}

int8_t qmc5883_select_pwr_mode(uint8_t pwr_mode, qmc5883_dev *dev)
{
    if (dev == NULL)
        return QMC5883_E_NULL_PTR;

    if (dev->chip_type == QMC5883L)
    {
        switch (pwr_mode)
        {
        case RT_SENSOR_POWER_DOWN: /* power down, means suspend mode */
            qmc5883_select_mode(suspend, dev);
            break;

        case RT_SENSOR_POWER_NORMAL: /* power normal */
            qmc5883_select_mode(normal, dev);
            qmc5883_select_odr(ODR_200Hz, dev);
            qmc5883L_default_set(dev);
            break;

        default:
            return -RT_ERROR;
        }
    }
    else if (dev->chip_type == QMC5883P)
    {
        switch (pwr_mode)
        {
        case RT_SENSOR_POWER_DOWN: /* power down, means suspend mode */
            qmc5883_select_mode(suspend, dev);
            break;

        case RT_SENSOR_POWER_NORMAL: /* power normal */
            qmc5883_select_mode(normal, dev);
            qmc5883_select_odr(ODR_200Hz, dev);
            break;

        case RT_SENSOR_POWER_LOW: /* power low means min odr and normal mode*/
            qmc5883_select_mode(normal, dev);
            qmc5883_select_odr(ODR_10Hz, dev);
            break;

        case RT_SENSOR_POWER_HIGH:
            qmc5883_select_mode(continus, dev);
            qmc5883_select_odr(ODR_200Hz, dev);
            break;

        default:
            return -RT_ERROR;
        }
    }

    return RT_EOK;
}

int8_t qmc5883_get_sensor_data(qmc5883_mag_data_t *data, qmc5883_dev *dev)
{
    int8_t rslt; /* Variable to define error */
    if (data == NULL || dev == NULL)
    {
        return QMC5883_E_NULL_PTR;
    }

    if (dev->chip_type == QMC5883L)
    {
        rt_kprintf("QMC5883L read: \n");
        rslt = qmc5883_get_regs(QMC5883L_MAG_X_LSB_ADDR, (uint8_t *)data, QMC5883_MAX_LEN, dev);
    }
    else if (dev->chip_type == QMC5883P)
    {
        rt_kprintf("QMC5883P read: \n");
        rslt = qmc5883_get_regs(QMC5883P_MAG_X_LSB_ADDR, (uint8_t *)data, QMC5883_MAX_LEN, dev);
    }

    return rslt;
}

static int8_t null_ptr_check(const qmc5883_dev *dev)
{
    if (!dev)
        return QMC5883_E_NULL_PTR;

    return QMC5883_OK;
}

static int8_t check_self_test(qmc5883_mag_data_t *before, qmc5883_mag_data_t *after)
{
    if (before == NULL || after == NULL)
        return QMC5883_E_NULL_PTR;

    /* check code... */

    return QMC5883_OK;
}

static int8_t qmc5883L_default_set(qmc5883_dev *dev)
{
    uint8_t qmc5883_set_reg_default_val = 0x01;

    if (!dev)
        return QMC5883_E_NULL_PTR;

    return qmc5883_set_regs(QMC5883L_SET_ADDR, &qmc5883_set_reg_default_val, 1, dev);
}
